feat(control): port temporal trajectory follower and follow-ups from X2 by Owen-Liuyuxuan · Pull Request #3371 · tier4/autoware_universe

Owen-Liuyuxuan · 2026-09-04T01:26:26Z

Summary

Port temporal MPC/PID/follower reference-mode from X2 (trajectory_reference_mode: resample by time, not arc length).
Include the X2 follow-ups that temporal mode depends on (not optional extras after #2957):
- PID velocity look-ahead (#3067)
- Taubin curvature for short sequences (#3041)
- MPC stop-hold / confidence steer slew + LPF sync (#3134, #3172)
- Trajectory-steer feedforward via MPCTrajectory.steer (#3268)
C++ / package YAML default remains spatial for standalone tests; E2E opts in via launcher #2190.

Why these land together

Temporal following without the later X2 patches snaps steer on spatially collapsed DP trajs, mis-estimates curvature on short horizons, and treats hesitation as a stop. Modifier/optimizer plugin flags that rewrite time_from_start are paired on the launcher PR, not isolated from this change.

Test plan

colcon build --packages-select autoware_mpc_lateral_controller autoware_pid_longitudinal_controller autoware_trajectory_follower_node
colcon test --packages-select autoware_mpc_lateral_controller autoware_pid_longitudinal_controller
CI on this PR
Pair with launcher temporal YAML — https://github.com/tier4/autoware_launch.xx1/pull/2190
Vehicle/runtime — not in this pass

Notes for reviewers

PID ff_scale_min/max (#2948) was already on XX1.
Elapsed-time origin (#3138) is in the first temporal commit; do not re-apply #3050/#3072 as originally written.
Not ported: steering_direct_passthrough / MPPI skip-QP (X2 default false).
Source chain on feat/v0.64/e2e: #2957, #3041, #3067, #3134, #3138, #3172, #3268.

Stack

Checker — feat(traffic_light_compliance_checker): copy package from X2 e2e #3368
TrafficLightStop — feat(trajectory_modifier): adapt TrafficLightStop from X2 processor #3369
boundary-departure — feat(planning_validator): add uncrossable-boundary departure plugin #3370
this PR — temporal controller + follow-ups

Depends on #3370. Merge from the bottom.

Use timestamps for nearest-point selection in MPC and PID when trajectory_reference_mode is temporal. Default remains spatial in C++ so existing tests without time_from_start still run. Source: #2957 (d287a8e), AWF#12886 (58433ba), #3138 (44d891d), #3072 (7831873). Skipped Taubin (#3041) and trajectory-steer feedforward (#3268) due to XX1 MPCTrajectory API drift. Co-authored-by: Cursor <cursoragent@cursor.com> Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com>

mergify · 2026-09-04T01:27:03Z

Tick the box to add this pull request to the merge queue (same as @mergifyio queue).

Queue this pull request

Owen-Liuyuxuan · 2026-09-04T01:51:45Z

Two changes are needed to keep this temporal-controller port scoped and regression-resistant.

[P2] Keep the Universe package default spatial. control/autoware_trajectory_follower_node/param/trajectory_follower_node.param.yaml:5 changes the package-level default to temporal. The exact X2 Universe source keeps this value spatial; launch PR #2190 already opts the XX1 E2E configuration into temporal mode. Changing both places silently changes standalone and non-E2E users of the package config, and temporal validation can reject legacy trajectories without strictly increasing timestamps. Please leave the Universe YAML at spatial and make #2190 the scoped E2E override, consistent with the PR summary.

Add the temporal unit coverage from X2. The port adds substantial interpolation/resampling behavior but does not copy the relevant source tests: MPC temporal convertToMPCTrajectory coverage (including short segments), and PID lerpTrajectoryPointByTime stop-hold/restart behavior. Those tests exercise the central semantics and boundary cases of this PR and should migrate with the implementation.

The explicitly omitted Taubin smoothing and trajectory-steer feedforward are not core temporal correctness blockers: Taubin mainly improves path/noise robustness, while steer feedforward requires extending XX1's MPCTrajectory representation. They should be tracked as quality/performance follow-ups rather than silently treated as migrated.

Include the three-point stop helper from the lower stack PRs. Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> Co-authored-by: Cursor <cursoragent@cursor.com>

E2E opts into temporal via autoware_launch.xx1. Leave the Universe package YAML at spatial so standalone/non-E2E users are unchanged. Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> Co-authored-by: Cursor <cursoragent@cursor.com>

Temporal mode needs look-ahead PID, Taubin short-horizon curvature, confidence steer hold, and trajectory-steer feedforward. These land with the temporal controller, not as isolated extras. Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> Co-authored-by: Cursor <cursoragent@cursor.com>

N-Iemura · 2026-09-07T03:34:01Z

Test result

I tested this changes with psim.

export VEHICLE_ID=5
ros2 launch autoware_launch planning_simulator.launch.xml map_path:=$HOME/autoware_map/odaiba_stable vehicle_model:=jpntaxi sensor_model:=aip_xx1_gen2 planning_setting:=diffusion_planner

✅ Working correctly

Planning Simulator started successfully with Diffusion Planner and Japantaxi 5.
The vehicle could enter AUTO mode and drive on straight sections.

❌ Problem found

The vehicle often failed to follow curves as shown in the attached video.
- The controller repeatedly switched between DRIVE and STOPPING near curves.

Investigation

The suspected cause was the coincident trajectory points preserved by remove_close_points=false.

Diffusion Planner generates stop points at the same position with different time_from_start values. These points create a zero-length segment in calcStopDistance(). The calculation then returns NaN, which is converted to a stop distance of 0.0, causing the controller to incorrectly enter the STOPPING state.

To verify this, I changed only remove_close_points to true and drove the same route again.

`remove_close_points=false`	`remove_close_points=true`
The vehicle often failed to follow curves.	The vehicle followed the same curves successfully.
before.webm	after.webm

This problem is expected to be resolved by updating planning pipeline to XX1 maybe

Owen-Liuyuxuan · 2026-09-07T04:06:12Z

@N-Iemura I think in X2 planner, we are using remove_close_points: false, is there any mechanism in Controller setting that make controller failed?

Bring in velocity modifier from #3379 now that it landed on the boundary-departure base. Merge instead of rebase: universe blocks force-push.

sonarqubecloud · 2026-09-07T08:01:08Z

Quality Gate passed

Issues
17 New issues
0 Accepted issues

Measures
0 Security Hotspots
0.0% Coverage on New Code
0.0% Duplication on New Code

See analysis details on SonarQube Cloud

…3370) * feat(planning_validator): add uncrossable-boundary departure plugin Reject the single planned trajectory on CRITICAL road-border departure and publish diagnostic boundary_departure for MRM. Source: X2 autoware_trajectory_validator uncrossable_boundary_departure d1ae5d0 (#3358), mapped onto planning_validator PluginInterface. Co-authored-by: Cursor <cursoragent@cursor.com> Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> * feat(trajectory_modifier): port velocity modifier from X2 (#3379) feat(trajectory_modifier): enable velocity modifier * feat(control): port temporal trajectory follower and follow-ups from X2 (#3371) * feat(control): port temporal trajectory_reference_mode from X2 Use timestamps for nearest-point selection in MPC and PID when trajectory_reference_mode is temporal. Default remains spatial in C++ so existing tests without time_from_start still run. Source: #2957 (d287a8e), AWF#12886 (58433ba), #3138 (44d891d), #3072 (7831873). Skipped Taubin (#3041) and trajectory-steer feedforward (#3268) due to XX1 MPCTrajectory API drift. Co-authored-by: Cursor <cursoragent@cursor.com> Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> * fix(control): keep follower package default spatial E2E opts into temporal via autoware_launch.xx1. Leave the Universe package YAML at spatial so standalone/non-E2E users are unchanged. Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> Co-authored-by: Cursor <cursoragent@cursor.com> * feat(control): port temporal MPC/PID follow-ups from X2 Temporal mode needs look-ahead PID, Taubin short-horizon curvature, confidence steer hold, and trajectory-steer feedforward. These land with the temporal controller, not as isolated extras. Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> Co-authored-by: Cursor <cursoragent@cursor.com> --------- Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> Co-authored-by: Cursor <cursoragent@cursor.com> --------- Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> Co-authored-by: Cursor <cursoragent@cursor.com> Co-authored-by: iemura <iemura-n1207@keio.jp>

…3369) * feat(trajectory_modifier): adapt TrafficLightStop from X2 processor Extend InputData with map/route/TL and register a modifier plugin using modify_trajectory (not processor process()). Source: #3017 (0bc9698), AWF#12875 (502d264), Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> Co-authored-by: Cursor <cursoragent@cursor.com> #3091 (9fc3824), #3232 (f5f933e). * fix(trajectory_modifier): emit three-point temporal-safe stop trajectory Immediate-stop replacement used two identical zero-time points, which temporal PID/MPC reject. Match X2: three points at 0/dt/2dt with 1 mm offsets, and pass trajectory_time_step from all stop plugins. Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> Co-authored-by: Cursor <cursoragent@cursor.com> * feat(planning_validator): add uncrossable-boundary departure plugin (#3370) * feat(planning_validator): add uncrossable-boundary departure plugin Reject the single planned trajectory on CRITICAL road-border departure and publish diagnostic boundary_departure for MRM. Source: X2 autoware_trajectory_validator uncrossable_boundary_departure d1ae5d0 (#3358), mapped onto planning_validator PluginInterface. Co-authored-by: Cursor <cursoragent@cursor.com> Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> * feat(trajectory_modifier): port velocity modifier from X2 (#3379) feat(trajectory_modifier): enable velocity modifier * feat(control): port temporal trajectory follower and follow-ups from X2 (#3371) * feat(control): port temporal trajectory_reference_mode from X2 Use timestamps for nearest-point selection in MPC and PID when trajectory_reference_mode is temporal. Default remains spatial in C++ so existing tests without time_from_start still run. Source: #2957 (d287a8e), AWF#12886 (58433ba), #3138 (44d891d), #3072 (7831873). Skipped Taubin (#3041) and trajectory-steer feedforward (#3268) due to XX1 MPCTrajectory API drift. Co-authored-by: Cursor <cursoragent@cursor.com> Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> * fix(control): keep follower package default spatial E2E opts into temporal via autoware_launch.xx1. Leave the Universe package YAML at spatial so standalone/non-E2E users are unchanged. Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> Co-authored-by: Cursor <cursoragent@cursor.com> * feat(control): port temporal MPC/PID follow-ups from X2 Temporal mode needs look-ahead PID, Taubin short-horizon curvature, confidence steer hold, and trajectory-steer feedforward. These land with the temporal controller, not as isolated extras. Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> Co-authored-by: Cursor <cursoragent@cursor.com> --------- Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> Co-authored-by: Cursor <cursoragent@cursor.com> --------- Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> Co-authored-by: Cursor <cursoragent@cursor.com> Co-authored-by: iemura <iemura-n1207@keio.jp> --------- Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> Co-authored-by: Cursor <cursoragent@cursor.com> Co-authored-by: iemura <iemura-n1207@keio.jp>

* feat(traffic_light_compliance_checker): copy package from X2 e2e Port the checker library so TrafficLightStop can run on XX1 modifier. Source: #3009 (5d7ab5b), #3232 (f5f933e), copied tree #3346 (cd44e12). Co-authored-by: Cursor <cursoragent@cursor.com> Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> * feat(trajectory_modifier): adapt TrafficLightStop from X2 processor (#3369) * feat(trajectory_modifier): adapt TrafficLightStop from X2 processor Extend InputData with map/route/TL and register a modifier plugin using modify_trajectory (not processor process()). Source: #3017 (0bc9698), AWF#12875 (502d264), Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> Co-authored-by: Cursor <cursoragent@cursor.com> #3091 (9fc3824), #3232 (f5f933e). * fix(trajectory_modifier): emit three-point temporal-safe stop trajectory Immediate-stop replacement used two identical zero-time points, which temporal PID/MPC reject. Match X2: three points at 0/dt/2dt with 1 mm offsets, and pass trajectory_time_step from all stop plugins. Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> Co-authored-by: Cursor <cursoragent@cursor.com> * feat(planning_validator): add uncrossable-boundary departure plugin (#3370) * feat(planning_validator): add uncrossable-boundary departure plugin Reject the single planned trajectory on CRITICAL road-border departure and publish diagnostic boundary_departure for MRM. Source: X2 autoware_trajectory_validator uncrossable_boundary_departure d1ae5d0 (#3358), mapped onto planning_validator PluginInterface. Co-authored-by: Cursor <cursoragent@cursor.com> Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> * feat(trajectory_modifier): port velocity modifier from X2 (#3379) feat(trajectory_modifier): enable velocity modifier * feat(control): port temporal trajectory follower and follow-ups from X2 (#3371) * feat(control): port temporal trajectory_reference_mode from X2 Use timestamps for nearest-point selection in MPC and PID when trajectory_reference_mode is temporal. Default remains spatial in C++ so existing tests without time_from_start still run. Source: #2957 (d287a8e), AWF#12886 (58433ba), #3138 (44d891d), #3072 (7831873). Skipped Taubin (#3041) and trajectory-steer feedforward (#3268) due to XX1 MPCTrajectory API drift. Co-authored-by: Cursor <cursoragent@cursor.com> Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> * fix(control): keep follower package default spatial E2E opts into temporal via autoware_launch.xx1. Leave the Universe package YAML at spatial so standalone/non-E2E users are unchanged. Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> Co-authored-by: Cursor <cursoragent@cursor.com> * feat(control): port temporal MPC/PID follow-ups from X2 Temporal mode needs look-ahead PID, Taubin short-horizon curvature, confidence steer hold, and trajectory-steer feedforward. These land with the temporal controller, not as isolated extras. Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> Co-authored-by: Cursor <cursoragent@cursor.com> --------- Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> Co-authored-by: Cursor <cursoragent@cursor.com> --------- Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> Co-authored-by: Cursor <cursoragent@cursor.com> Co-authored-by: iemura <iemura-n1207@keio.jp> --------- Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> Co-authored-by: Cursor <cursoragent@cursor.com> Co-authored-by: iemura <iemura-n1207@keio.jp> --------- Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> Co-authored-by: Cursor <cursoragent@cursor.com> Co-authored-by: iemura <iemura-n1207@keio.jp>

* feat(traffic_light_compliance_checker): copy package from X2 e2e Port the checker library so TrafficLightStop can run on XX1 modifier. Source: #3009 (5d7ab5b), #3232 (f5f933e), copied tree #3346 (cd44e12). Co-authored-by: Cursor <cursoragent@cursor.com> Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> * feat(trajectory_modifier): adapt TrafficLightStop from X2 processor (#3369) * feat(trajectory_modifier): adapt TrafficLightStop from X2 processor Extend InputData with map/route/TL and register a modifier plugin using modify_trajectory (not processor process()). Source: #3017 (0bc9698), AWF#12875 (502d264), Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> Co-authored-by: Cursor <cursoragent@cursor.com> #3091 (9fc3824), #3232 (f5f933e). * fix(trajectory_modifier): emit three-point temporal-safe stop trajectory Immediate-stop replacement used two identical zero-time points, which temporal PID/MPC reject. Match X2: three points at 0/dt/2dt with 1 mm offsets, and pass trajectory_time_step from all stop plugins. Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> Co-authored-by: Cursor <cursoragent@cursor.com> * feat(planning_validator): add uncrossable-boundary departure plugin (#3370) * feat(planning_validator): add uncrossable-boundary departure plugin Reject the single planned trajectory on CRITICAL road-border departure and publish diagnostic boundary_departure for MRM. Source: X2 autoware_trajectory_validator uncrossable_boundary_departure d1ae5d0 (#3358), mapped onto planning_validator PluginInterface. Co-authored-by: Cursor <cursoragent@cursor.com> Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> * feat(trajectory_modifier): port velocity modifier from X2 (#3379) feat(trajectory_modifier): enable velocity modifier * feat(control): port temporal trajectory follower and follow-ups from X2 (#3371) * feat(control): port temporal trajectory_reference_mode from X2 Use timestamps for nearest-point selection in MPC and PID when trajectory_reference_mode is temporal. Default remains spatial in C++ so existing tests without time_from_start still run. Source: #2957 (d287a8e), AWF#12886 (58433ba), #3138 (44d891d), #3072 (7831873). Skipped Taubin (#3041) and trajectory-steer feedforward (#3268) due to XX1 MPCTrajectory API drift. Co-authored-by: Cursor <cursoragent@cursor.com> Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> * fix(control): keep follower package default spatial E2E opts into temporal via autoware_launch.xx1. Leave the Universe package YAML at spatial so standalone/non-E2E users are unchanged. Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> Co-authored-by: Cursor <cursoragent@cursor.com> * feat(control): port temporal MPC/PID follow-ups from X2 Temporal mode needs look-ahead PID, Taubin short-horizon curvature, confidence steer hold, and trajectory-steer feedforward. These land with the temporal controller, not as isolated extras. Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> Co-authored-by: Cursor <cursoragent@cursor.com> --------- Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> Co-authored-by: Cursor <cursoragent@cursor.com> --------- Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> Co-authored-by: Cursor <cursoragent@cursor.com> Co-authored-by: iemura <iemura-n1207@keio.jp> --------- Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> Co-authored-by: Cursor <cursoragent@cursor.com> Co-authored-by: iemura <iemura-n1207@keio.jp> --------- Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> Co-authored-by: Cursor <cursoragent@cursor.com> Co-authored-by: iemura <iemura-n1207@keio.jp> (cherry picked from commit 145e799)

Owen-Liuyuxuan requested review from PanConChicharron, maxime-clem, mkquda, yuki-takagi-66 and zulfaqar-azmi-t4 as code owners September 4, 2026 01:26

This was referenced Sep 4, 2026

feat(traffic_light_compliance_checker): copy package from X2 e2e #3368

Merged

feat(trajectory_modifier): adapt TrafficLightStop from X2 processor #3369

Merged

Owen-Liuyuxuan mentioned this pull request Sep 4, 2026

feat(planning_validator): add uncrossable-boundary departure plugin #3370

Merged

1 of 4 tasks

Owen-Liuyuxuan and others added 3 commits September 4, 2026 11:11

Merge feat/e2e-foundation-x2-port-boundary-departure into temporal stack

7c132dc

Include the three-point stop helper from the lower stack PRs. Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> Co-authored-by: Cursor <cursoragent@cursor.com>

Owen-Liuyuxuan changed the title ~~feat(control): port temporal trajectory_reference_mode from X2~~ feat(control): port temporal trajectory follower and follow-ups from X2 Sep 4, 2026

KYabuuchi requested a review from N-Iemura September 4, 2026 06:42

N-Iemura mentioned this pull request Sep 7, 2026

feat(trajectory_modifier): port velocity modifier from X2 #3379

Merged

2 of 4 tasks

Merge feat/e2e-foundation-x2-port-boundary-departure into temporal stack

5dce406

Bring in velocity modifier from #3379 now that it landed on the boundary-departure base. Merge instead of rebase: universe blocks force-push.

Owen-Liuyuxuan merged commit 872f5e2 into feat/e2e-foundation-x2-port-boundary-departure Sep 8, 2026
4 checks passed

Navigation Menu

Repository navigation

feat(control): port temporal trajectory follower and follow-ups from X2 - #3371

feat(control): port temporal trajectory follower and follow-ups from X2#3371
Owen-Liuyuxuan merged 5 commits into
feat/e2e-foundation-x2-port-boundary-departurefrom
feat/e2e-foundation-x2-port-temporal-controller

Owen-Liuyuxuan commented Sep 4, 2026 •

edited

Loading

Uh oh!

mergify Bot commented Sep 4, 2026

Uh oh!

Owen-Liuyuxuan commented Sep 4, 2026

Uh oh!

N-Iemura commented Sep 7, 2026

Uh oh!

Owen-Liuyuxuan commented Sep 7, 2026

Uh oh!

sonarqubecloud Bot commented Sep 7, 2026

Uh oh!

Uh oh!

Reviewers

Uh oh!

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

Conversation

Owen-Liuyuxuan commented Sep 4, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Summary

Why these land together

Test plan

Notes for reviewers

Stack

Uh oh!

mergify Bot commented Sep 4, 2026

Uh oh!

Owen-Liuyuxuan commented Sep 4, 2026

Uh oh!

N-Iemura commented Sep 7, 2026

Test result

✅ Working correctly

❌ Problem found

Investigation

Uh oh!

Owen-Liuyuxuan commented Sep 7, 2026

Uh oh!

sonarqubecloud Bot commented Sep 7, 2026

Quality Gate passed

Uh oh!

Uh oh!

Reviewers

Uh oh!

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

Owen-Liuyuxuan commented Sep 4, 2026 •

edited

Loading